#include "Motor.h"

/**
 * @brief 左电机控制
 * @param PWM PWM值 (-1000 ~ 1000)
 */
void Motor_Left(int PWM)
{
    
    if(PWM > 0) {
        // 左轮正转
        DL_TimerA_setCaptureCompareValue(PWM_INST, 0, GPIO_PWM_C1_IDX);
        DL_TimerA_setCaptureCompareValue(PWM_INST, PWM, GPIO_PWM_C0_IDX);
    }
    else if(PWM < 0) {
        // 左轮反转
        DL_TimerA_setCaptureCompareValue(PWM_INST, -PWM, GPIO_PWM_C1_IDX);
        DL_TimerA_setCaptureCompareValue(PWM_INST, 0, GPIO_PWM_C0_IDX);
    }
    else {
        // 左轮停止
        DL_TimerA_setCaptureCompareValue(PWM_INST, 0, GPIO_PWM_C1_IDX);
        DL_TimerA_setCaptureCompareValue(PWM_INST, 0, GPIO_PWM_C0_IDX);
    }
}

/**
 * @brief 右电机控制
 * @param PWM PWM值 (-1000 ~ 1000)
 */
void Motor_Right(int PWM)
{
    
    if(PWM > 0) {
        // 右轮正转
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, 0, GPIO_PWMyou_C1_IDX);
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, PWM, GPIO_PWMyou_C0_IDX);
    }
    else if(PWM < 0) {
        // 右轮反转
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, -PWM, GPIO_PWMyou_C1_IDX);
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, 0, GPIO_PWMyou_C0_IDX);
    }
    else {
        // 右轮停止
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, 0, GPIO_PWMyou_C1_IDX);
        DL_TimerG_setCaptureCompareValue(PWMyou_INST, 0, GPIO_PWMyou_C0_IDX);
    }
}

void Motor_Left_go(void) //左轮电机反转
{
    //AIN1_OUT(1);
    DL_TimerA_setCaptureCompareValue(PWM_INST,1000,GPIO_PWM_C1_IDX);
   // AIN2_OUT(0);
    DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C0_IDX);
}

void  Motor_Left_back(void) //左轮电机正转
{
    //AIN1_OUT(0);
    DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C1_IDX);
    //AIN2_OUT(1);
    DL_TimerA_setCaptureCompareValue(PWM_INST,1000,GPIO_PWM_C0_IDX);
}

void Motor_Right_go(void) //右轮电机正转
{
    //DIN1_OUT(1);
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,1000,GPIO_PWMyou_C1_IDX);
    //DIN2_OUT(0);
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C0_IDX);
}


void Motor_Right_back(void) //右轮电机反转
{
    //DIN1_OUT(0);
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C1_IDX);
    //DIN2_OUT(1);
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,1000,GPIO_PWMyou_C0_IDX);
}


/**
 * @brief: 控制电机停止
 * @param Num 0:全部停止 1:A电机停止 2:B电机停止
 * @retval: 
 */
void Motor_Stop(uint8_t Num)
{
    switch (Num)
    {
        case 1: //左电机停止
            //DIN1_OUT(0);
            DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C1_IDX);
            //DIN2_OUT(0);
            DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C0_IDX);
            break;
        case 2: //右电机停止
            //AIN1_OUT(0);
            DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C1_IDX);
            //AIN2_OUT(0);
            DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C0_IDX); 
            break;   
        default:
            //AIN1_OUT(0);
            DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C1_IDX);
            //AIN2_OUT(0);
            DL_TimerG_setCaptureCompareValue(PWMyou_INST,0,GPIO_PWMyou_C0_IDX);
            //DIN1_OUT(0);
            DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C1_IDX);
            //DIN2_OUT(0);
            DL_TimerA_setCaptureCompareValue(PWM_INST,0,GPIO_PWM_C0_IDX);
            break;
    }
}



/**
 * @brief: 写入寄存器 输出占空比PWM  右电机
 * @param SpeedA:电机A1的PWM占空比输入值
 * @param SpeedD:电机B1的PWM占空比输入值
 * @retval: 
 */
void Motor_Pwm_Start(int SpeedA,int SpeedD)
{
    if(SpeedA == 1000){SpeedA = 999;}    if(SpeedD == 1000){SpeedD = 999;}
    DL_TimerA_setCaptureCompareValue(PWM_INST,SpeedA,GPIO_PWM_C0_IDX);
    DL_TimerA_setCaptureCompareValue(PWM_INST,SpeedD,GPIO_PWM_C1_IDX);   
}

/**
 * @brief: 写入寄存器 输出占空比PWM  左电机
 * @param SpeedA:电机A2的PWM占空比输入值
 * @param SpeedD:电机B2的PWM占空比输入值
 * @retval: 
 */
void Motor_Pwmyou_Start(int SpeedB,int SpeedE)
{
    if(SpeedB == 1000){SpeedB = 999;}    if(SpeedE == 1000){SpeedE = 999;}
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,SpeedB,GPIO_PWMyou_C0_IDX);
    DL_TimerG_setCaptureCompareValue(PWMyou_INST,SpeedE,GPIO_PWMyou_C1_IDX);   
}


/**
 * @brief: 输入可控制旋转方向的PWM占空比 zuo
 * @param SpeedA:-1000 ~ 1000
 * @param SpeedD:-1000 ~ 1000
 * @param SpeedB:-1000 ~ 1000
 * @param SpeedE:-1000 ~ 1000
 * @retval: 
 */
// void Motor_Control(int SpeedA, int SpeedD , int SpeedB , int SpeedE)
// {
//     uint16_t DataA = (SpeedA < 0) ? -SpeedA : SpeedA;
//     uint16_t DataD = (SpeedD < 0) ? -SpeedD : SpeedD;
//     uint16_t DataB = (SpeedB < 0) ? -SpeedB : SpeedB;
//     uint16_t DataE = (SpeedE < 0) ? -SpeedE : SpeedE;
//     // 处理zuo电机的前进、后退或停止
//     if (SpeedA > 0 && SpeedA > SpeedD) {
//         Motor_Left_go();
//     } else if (SpeedD > 0 && SpeedD > SpeedA) {
//         Motor_Left_back();
//     } else {
//         Motor_Stop(1);  // zuo电机停止
//         DataD = 0;          // zuo电机数据设置为 0
//         DataA = 0;
//     }

//     // 如果电机的速度都为 0，则停止所有电机
//     if (SpeedA == 0 && SpeedD == 0) {
//         Motor_Stop(1); // 所有电机停止
//         Motor_Pwm_Start(0, 0);
//     } else {
//         // 启动电机 PWM
//         Motor_Pwm_Start(DataA, DataD);
//     }

//         // 处理右电机的前进、后退或停止
//     if (SpeedE > 0 && SpeedE > SpeedB) {
//         Motor_Right_go();
//     } else if (SpeedB > 0 && SpeedB > SpeedA) {
//         Motor_Right_back();
//     } else {
//         Motor_Stop(2);  // 右电机停止
//         DataD = 0;          // 右电机数据设置为 0
//         DataA = 0;
//     }

//     // 如果电机的速度都为 0，则停止所有电机
//     if (SpeedE == 0 && SpeedB == 0) {
//         Motor_Stop(2); // 所有电机停止
//         Motor_Pwmyou_Start(0, 0);
//     } else {
//         // 启动电机 PWM
//         Motor_Pwmyou_Start(DataB, DataE);
//     }

    
// }

void Motor_Control(int PWMA ,int PWMB)
{
    Motor_Left(PWMA);
    Motor_Right(PWMB);
}


/**
 * @brief: 设置电机速度 单位 cm/s
 * @param  范围 -180 ~ 180 cm/s
 * @retval: 
 */
void Motor_Set_Speed(float Left_Speed,float Right_Speed)
{
        // 限制速度在有效范围内
    PIDw_Left_Motor.Target  = Left_Speed;
    PIDw_Right_Motor.Target  = Right_Speed;
    PIDz_Left_Motor.Target  = Left_Speed;
    PIDz_Right_Motor.Target  = Right_Speed;

}